"""

This Circuit Python code to control a Robot Quadruped "PICO Python Dog" can be freely used and modified,

except for commercial and provided this 6 line comment remains in the code at the top of the .py file

and these 6 lines are not modified or removed.

"""

import time

import board

import busio

import pwmio

from adafruit_motor import servo

import adafruit_icm20x

from math import asin, acos, atan, degrees, sqrt

import pulseio

import adafruit_irremote

# IR sensor

IR_PIN = board.GP9 # Pin GP9 connected to IR receiver

# IR decoder

pulsein = pulseio.PulseIn(IR_PIN, maxlen=100, idle_state=True)

decoder = adafruit_irremote.GenericDecode()

pulsein.clear()

pulsein.resume()

# Acc Gyro Mag test setup on i2c (1)

i2c = busio.I2C(board.GP11, board.GP10)

icm = adafruit_icm20x.ICM20948(i2c)

# AGM functions

def Acc(): # Acc +/- 10 for 90 degrees of x and y axis (wobble board +...)

Ax = icm.acceleration[0] # Gyro detects sudden movement in real time (compliance +...)

Ay = icm.acceleration[1] # Mag (compass) for up side down detection

return Ax, Ay

# Servo setup (Raspberry Pi PICO micro controller (Circuit Python) with Tower Pro MG90S servos

FL_knee = servo.Servo(pwmio.PWMOut(board.GP0, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FL_hip = servo.Servo(pwmio.PWMOut(board.GP1, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FR_knee = servo.Servo(pwmio.PWMOut(board.GP27, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FR_hip = servo.Servo(pwmio.PWMOut(board.GP26, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BL_knee = servo.Servo(pwmio.PWMOut(board.GP14, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BL_hip = servo.Servo(pwmio.PWMOut(board.GP15, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BR_knee = servo.Servo(pwmio.PWMOut(board.GP21, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BR_hip = servo.Servo(pwmio.PWMOut(board.GP20, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

# servo function - converts height, slew, elevation and roll inputs to degrees of angles of servos

def servo_drive(height, slew, elev, roll):

if height > 95: height = 95

if height < 5: height = 5

if slew < -33: slew = -33

if slew > 33: slew = 33

if elev < -20: elev = -20

if elev > 20: elev = 20

if roll < -20: roll = -15

if roll > 20: roll = 15

if elev != 0:

F_height = height - elev

B_height = height + elev

hip_offset = asin(elev / 49)

else:

F_height = height

B_height = height

hip_offset = 0

F_height_slew = sqrt((F_height * F_height) + (slew * slew))

B_height_slew = sqrt((B_height * B_height) + (slew * slew))

if roll != 0:

F_L_height = F_height_slew - roll

F_R_height = F_height_slew + roll

B_L_height = B_height_slew - roll

B_R_height = B_height_slew + roll

else:

F_L_height = F_height_slew

F_R_height = F_height_slew

B_L_height = B_height_slew

B_R_height = B_height_slew

F_L_knee = acos(1 - ((F_L_height * F_L_height) / 5000))

F_R_knee = acos(1 - ((F_R_height * F_R_height) / 5000))

B_L_knee = acos(1 - ((B_L_height * B_L_height) / 5000))

B_R_knee = acos(1 - ((B_R_height * B_R_height) / 5000))

F_L_hip = (F_L_knee / 2) + atan(slew / F_L_height) + hip_offset

F_R_hip = (F_R_knee / 2) + atan(slew / F_R_height) + hip_offset

B_L_hip = (B_L_knee / 2) + atan(slew / B_L_height) + hip_offset

B_R_hip = (B_R_knee / 2) + atan(slew / B_R_height) + hip_offset

F_L_hip = degrees(F_L_hip)

F_R_hip = degrees(F_R_hip)

F_L_knee = degrees(F_L_knee)

F_R_knee = degrees(F_R_knee)

B_L_hip = degrees(B_L_hip)

B_R_hip = degrees(B_R_hip)

B_L_knee = degrees(B_L_knee)

B_R_knee = degrees(B_R_knee)

if F_L_hip > 175: F_L_hip = 175

if F_R_hip > 175: F_R_hip = 175

if B_L_hip > 175: B_L_hip = 175

if B_R_hip > 175: B_R_hip = 175

if F_L_hip < 5: F_L_hip = 5

if F_R_hip < 5: F_R_hip = 5

if B_L_hip < 5: B_L_hip = 5

if B_R_hip < 5: B_R_hip = 5

if F_L_knee > 175: F_L_knee = 175

if F_R_knee > 175: F_R_knee = 175

if B_L_knee > 175: B_L_knee = 175

if B_R_knee > 175: B_R_knee = 175

if F_L_knee < 5: F_L_knee = 5

if F_R_knee < 5: F_R_knee = 5

if B_L_knee < 5: B_L_knee = 5

if B_R_knee < 5: B_R_knee = 5

FL_knee.angle = 180 - F_L_knee

FL_hip.angle = 180 - F_L_hip

FR_knee.angle = F_R_knee

FR_hip.angle = F_R_hip

BL_knee.angle = 180 - B_L_knee

BL_hip.angle = 180 - B_L_hip

BR_knee.angle = B_R_knee

BR_hip.angle = B_R_hip

""" MAIN CODE """

time_delay = 0.3

scale = 1

h = 0 # starting laying down

s = 0 # starting with feet direcly under hips

e = 0 # starting with body level on elevation

r = 0 # starting with body level on roll

time.sleep(time_delay *2) # start delay

for h_bar in range(0, 650, scale): # start position from lay

h=h_bar/10

servo_drive(h, s, e, r)

time.sleep(time_delay / 100)

time.sleep(time_delay *1)

h = 65

while True:

Ax, Ay = Acc()

if Ax > 5: e += 4 # Needs fine adjustment with fractions

elif Ax > 4: e += 3

elif Ax > 3: e += 2

elif Ax > 2: e += 1

elif Ax > 1: e += 0.5

elif Ax < -5: e -= 4

elif Ax < -4: e -= 3

elif Ax < -3: e -= 2

elif Ax < -2: e -= 1

elif Ax < -1: e -= 0.5

if Ay > 2: r += 1

elif Ay > 1: r += 0.5

elif Ay <-2: r -= 1

elif Ay <-1: r -= 0.5

if h > 95: h = 95

if h < 5: h = 5

if s < -33: s = -33

if s > 33: s = 33

if e < -20: e = -20

if e > 20: e = 20

if r < -20: r = -15

if r > 20: r = 15

servo_drive(h, s, e, r)